﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox
{
    public class BoxShape : PhysicsObjectShapeBase
    {
        public Vector3 center;
        public Vector3 size = new Vector3(1, 1, 1);


        public override bool IsHit(Vector3 compareMassPoint)
        {
            var localCompareMassPoint = transform.worldToLocalMatrix.MultiplyPoint3x4(compareMassPoint);

            var sizeHalf = size * 0.5f;

            if (localCompareMassPoint.x <= center.x + sizeHalf.x && localCompareMassPoint.x >= center.x - sizeHalf.x)
            {
                if (localCompareMassPoint.y <= center.y + sizeHalf.y && localCompareMassPoint.y >= center.y - sizeHalf.y)
                {
                    if (localCompareMassPoint.z <= center.z + sizeHalf.z && localCompareMassPoint.z >= center.z - sizeHalf.z)
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        public override bool IsHit(PhysicsObjectShapeBase shape, out Vector3 referencePoint)
        {
            if (shape is CapsuleShape)
            {
                return PhysicsUtil.CapsuleHitOther(shape as CapsuleShape, this, out referencePoint);
            }
            else
            {
                var edgePoint = shape.MassPointToEdge(shape.GetWorldPosition(), GetWorldPosition());
                referencePoint = edgePoint;
                return IsHit(edgePoint);
            }
        }

        public override Vector3 MassPointToEdge(Vector3 massPoint, Vector3 outsidePoint)
        {
            var result = Vector3.one * PhysicsUtil.Infinity;
            var error = Vector2.one * 0.01f;
            var hasValue = false;

            var relativePosition = transform.position + center + transform.rotation * (Vector3.forward * size.z * 0.5f);
            var normal = transform.rotation * Vector3.forward;
            var rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            var intersectPoint = GetIntersectPoint(Vector3.forward, relativePosition, massPoint, outsidePoint, rect);
            if (intersectPoint != null) result = intersectPoint.Value; hasValue = true;
            //-----------Forward.

            relativePosition = transform.position + center + transform.rotation * (Vector3.back * size.z * 0.5f);
            normal = transform.rotation * Vector3.back;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, massPoint, outsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Back.

            relativePosition = transform.position + center + transform.rotation * (Vector3.left * size.x * 0.5f);
            normal = transform.rotation * Vector3.left;
            rect = new Rect(Vector2.zero, new Vector2(size.z, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, massPoint, outsidePoint, rect);

            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Left.

            relativePosition = transform.position + center + transform.rotation * (Vector3.right * size.x * 0.5f);
            normal = transform.rotation * Vector3.right;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, massPoint, outsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Right.

            relativePosition = transform.position + center + transform.rotation * (Vector3.up * size.y * 0.5f);
            normal = transform.rotation * Vector3.up;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, massPoint, outsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Up.

            relativePosition = transform.position + center + transform.rotation * (Vector3.down * size.y * 0.5f);
            normal = transform.rotation * Vector3.down;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, massPoint, outsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Down.

            if (hasValue) return result;
            else throw new System.Exception("质点在Box外!");
        }

        public override Vector3 GetWorldPosition()
        {
            return transform.localToWorldMatrix.MultiplyPoint3x4(center);
        }

        protected override void DrawGizmos()
        {
            var cacheColor = Gizmos.color;

            Gizmos.color = Color.Lerp(Color.green, Color.gray, 0.5f);
            Gizmos.DrawWireCube(GetWorldPosition(), size);
            Gizmos.color = cacheColor;
        }

        Vector3? GetIntersectPoint(Vector3 planeNormal, Vector3 planePosition, Vector3 p0, Vector3 p1, Rect rect)
        {
            var sign1 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p0));
            var sign2 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p1));
            if (Mathf.Approximately(sign1, sign2)) return null;//同侧异侧.

            var a = planeNormal.x;
            var b = planeNormal.y;
            var c = planeNormal.z;
            var d = -a * planePosition.x - b * planePosition.y - c * planePosition.z;

            var i0 = a * p0.x + b * p0.y + c * p0.z;
            var i1 = a * p1.x + b * p1.y + c * p1.z;
            var final_t = -(i1 + d) / (i0 - i1);

            var finalPoint = new Vector3()
            {
                x = p0.x * final_t + p1.x * (1 - final_t),
                y = p0.y * final_t + p1.y * (1 - final_t),
                z = p0.z * final_t + p1.z * (1 - final_t),
            };

            var matrix = Matrix4x4.TRS(planePosition, Quaternion.FromToRotation(planeNormal, Vector3.forward), Vector3.one);
            var localPosition = matrix.inverse.MultiplyPoint3x4(finalPoint);

            if (localPosition.x <= rect.size.x * 0.5f
                 && localPosition.x >= -rect.size.x * 0.5f
                 && localPosition.y <= rect.size.y * 0.5f
                 && localPosition.y >= -rect.size.y * 0.5f)
            {
                return finalPoint;
            }
            else
            {
                return null;
            }
        }
    }
}
